(* # ===================================================================
   # Matrix Project
   # Copyright FEM-NUAA.CN 2020
   # =================================================================== *)


Require Import Reals.
Open Scope R_scope.
Require Export Matrix.Mat.RMatrix.
Require Export Matrix.Mat.RMtacs.
(** Sg -> Sb *)

(* 偏航角ᴪ   *)
Parameter psi  : R.

(* 俯仰角θ   *)
Parameter theta: R.

(* 滚动角Φ  *)
Parameter phi : R.


(* 由 地面坐标轴系Sg 转动 偏航角ᴪ  到 过度坐标轴系S’*)
Definition coordinate_transform_SgS' : Mat R 3 3 := mkMat_3_3
  (cos psi)  (sin psi) 0
  (-sin psi) (cos psi) 0
     0         0       1.

(* 由过度坐标轴系S’转动 俯仰角θ 到 过度坐标轴系S'' *)
Definition coordinate_transform_S'S'' : Mat R 3 3 := mkMat_3_3
  (cos theta)    0   (-sin theta)
       0         1        0
  (sin theta)    0   (cos theta).

(* 由过度坐标轴系S’’转动 滚动角Φ 到 机体坐标轴系Sb *)
Definition coordinate_transform_S''Sb : Mat R 3 3 := mkMat_3_3
  1    0          0
  0    (cos phi)  (sin phi)
  0    (-sin phi) (cos phi  ).

Definition coordinate_transform_SgSb : Mat R 3 3 := mkMat_3_3
  ((cos theta)*(cos psi))  ((cos theta)*(sin psi)) (-sin theta)
  ((sin theta)*(cos psi)*(sin phi)-(sin psi)*(cos phi))
  ((sin theta)*(sin psi)*(sin phi)+(cos psi)*(cos phi))
  ((cos theta)*(sin phi))
  ((sin theta)*(cos psi)*(cos phi)+(sin psi)*(sin phi))
  ((sin theta)*(sin psi)*(cos phi)-(cos psi)*(sin phi))
  ((cos theta)*(cos phi)).

Definition transition_S''Sb_mul_S'S'' : Mat R 3 3 := mkMat_3_3
  (cos theta)              0           (-sin theta)           
  ((sin phi)*(sin theta))  (cos phi)   ((sin phi)*(cos theta))
  ((cos phi)*(sin theta))  (-sin phi)  ((cos phi)*(cos theta)).

Lemma transition_S''Sb_mul_S'S''_eq:
  transition_S''Sb_mul_S'S'' === RMmul coordinate_transform_S''Sb coordinate_transform_S'S''.
Proof.
  unfold transition_S''Sb_mul_S'S''.
  RMat_mul_simpl. unfold mkMat_3_3'.
  f_equal3.
Qed.

(* S''Sb * S'S'' * SgS' = S''Sb_mul_S'S'' * SgS' *)
Definition transition_S''Sb_mul_S'S''_mul_SgS' :=
  RMmul transition_S''Sb_mul_S'S'' coordinate_transform_SgS'.


(* verify  S''Sb * S'S'' * SgS' = SgSb *)
Lemma coordinate_transform_SgSb_eq :
  coordinate_transform_SgSb === transition_S''Sb_mul_S'S''_mul_SgS'.
Proof.
  unfold coordinate_transform_SgSb.
  unfold transition_S''Sb_mul_S'S''_mul_SgS'.
  RMat_mul_simpl. unfold mkMat_3_3'.
  f_equal3. ring. f_equal2. ring. ring. ring.
  f_equal2. ring. ring. ring. f_equal. ring.
Qed.
